//********************************************************************************************
//*                                    C_SEN_01                                              *
//********************************************************************************************
//*    C_SEN_01                                                                              *
//*    version: 2.0                                                                          *
//*    The four obstacle sensors are checked.			                                     *
//*    Copyright (C) 2010  Bizintek Innova S.L.                                              *
//********************************************************************************************
//*    This program is free software; you can redistribute it and/or modify                  *
//*    it under the terms of the GNU General Public License as published by                  *
//*    the Free Software Foundation; either version 3 of the License, or                     *
//*    (at your option) any later version.                                                   *
//*                                                                                          *
//*    This program is distributed in the hope that it will be useful,                       *
//*    but WITHOUT ANY WARRANTY; without even the implied warranty of                        *
//*    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the                         *
//*    GNU General Public License for more details.                                          *
//*                                                                                          *
//*    You should have received a copy of the GNU General Public License along               *
//*    with this program; if not, write to the Free Software Foundation, Inc.,               *
//*    51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.                           *
//********************************************************************************************
// Fecha: 25/05/2010																		 *
//********************************************************************************************


#include "p18f86j50.h"  		//Moway microcontroller
#include "lib_sen_moway.h"		//Sensors library
#include "lib_mot_moway.h"		//Sensors library

//****************************************************************************************************************//
// JUMPING THE BOOTLOADER
//****************************************************************************************************************//
#define REMAPPED_RESET_VECTOR_ADDRESS			0x1000	//Reset adress for the correct bootloader jump
#define REMAPPED_HIGH_INTERRUPT_VECTOR_ADDRESS	0x1008	//High priority interruption adress for the correct bootloader jump
#define REMAPPED_LOW_INTERRUPT_VECTOR_ADDRESS	0x1018	//Low priority interruption adress for the correct bootloader jump


void YourHighPriorityISRCode();	//Function that executes the needed code in case of the high priority interruption jumps.
void YourLowPriorityISRCode();	//Function that executes the needed code in case of the low priority interruption jumps.


//*********************RESET, HIGH AND LOW PRIORITY INTERRUPTION REMAP****************************//
extern void _startup (void);    
#pragma code REMAPPED_RESET_VECTOR = REMAPPED_RESET_VECTOR_ADDRESS  
void _reset (void)
{
    _asm goto _startup _endasm  
}
#pragma code

#pragma code REMAPPED_HIGH_INTERRUPT_VECTOR = REMAPPED_HIGH_INTERRUPT_VECTOR_ADDRESS
void Remapped_High_ISR (void) { _asm goto YourHighPriorityISRCode _endasm }

#pragma code REMAPPED_LOW_INTERRUPT_VECTOR = REMAPPED_LOW_INTERRUPT_VECTOR_ADDRESS
void Remapped_Low_ISR (void) { _asm goto YourLowPriorityISRCode _endasm }

#pragma code

#pragma interrupt YourHighPriorityISRCode
void YourHighPriorityISRCode() {
    //high priority interrupt code
}

#pragma interruptlow YourLowPriorityISRCode
void YourLowPriorityISRCode() {
    //low priority interrupt code
}

//FORWARDS
void rotaSiColision();
void mueveAtras();
void avanzaNormal();


//************************************[MAIN]**************************************************
void main() {   
	MOT_CONFIG();
	SEN_CONFIG();
	while(1){
		rotaSiColision();
		avanzaNormal();
		rotaSiColision();
		avanzaNormal();
		rotaSiColision();
		avanzaNormal();
		rotaSiColision();
		avanzaNormal();
		rotaSiColision();
		avanzaNormal();
		rotaSiColision();
		avanzaNormal();
		rotaSiColision();
		mueveAtras();
	}
}

void rotaSiColision(){
	LED_FRONT_ON();
	LED_TOP_GREEN_ON();
	while ( SEN_OBS_ANALOG(OBS_CENTER_L)>100 ){
		while(!MOT_END){};
		MOT_ROT(80, FWD, CENTER, LEFT, ANGLE, 15) ;
		while(!MOT_END){};
	}
	LED_TOP_GREEN_OFF();
	LED_FRONT_OFF();
}

void mueveAtras() {
	LED_TOP_RED_ON();
	while(!MOT_END){};
	MOT_STR(65, BACK, DISTANCE, 15);
	while(!MOT_END){};
	LED_TOP_RED_OFF();
}

void avanzaNormal(){
	while(!MOT_END){};
	MOT_CUR(80, FWD, 20, RIGHT, TIME, 5) ;
	while(!MOT_END){};
}

/*
void aparcaSobreLaIzda() {
	LED_TOP_GREEN_ON();
	MOT_ROT(50, BACK, CENTER, LEFT, ANGLE, 25) ;
	while(!MOT_END){};
	MOT_STR(100, BACK,TIME,0);
	while(MOT_FDBCK(STATUS_T)[0]<5);
	MOT_STOP();
	LED_TOP_GREEN_OFF();	
}

void aparcaSobreLaDcha() {
	LED_TOP_RED_ON();
	MOT_ROT(50, BACK, CENTER, RIGHT, ANGLE, 25) ;
	while(!MOT_END){};
	MOT_STR(100, BACK,TIME,0);
	while(MOT_FDBCK(STATUS_T)[0]<5);
	MOT_STOP();
	LED_TOP_RED_OFF();	
}

void rectifica(unsigned char ultDerecha){
	if (SEN_OBS_ANALOG(OBS_SIDE_R) > ultDerecha){
		MOT_ROT(1, BACK, CENTER, RIGHT, ANGLE, 1) ;
		while(!MOT_END){};
	} else if (SEN_OBS_ANALOG(OBS_SIDE_R) < ultDerecha) {
		MOT_ROT(1, BACK, CENTER, LEFT, ANGLE, 1) ;
		while(!MOT_END){};
	}
	ultDerecha = SEN_OBS_ANALOG(OBS_SIDE_R);
}


unsigned char esSituacionBloqueo();
void accionSituacionBloqueo();
unsigned char esSituacionParedDcha();
void accionSituacionParedDcha();
unsigned char esSituacionParedIzda();
void accionSituacionParedIzda();
unsigned char esSituacionAvanzar();
void accionSituacionAvanzar();



void xavi() {
	unsigned char ultDerecha = SEN_OBS_ANALOG(OBS_SIDE_R);
	LED_FRONT_ON();
	while(1){
		//avanzaHasta(10);
		//rectifica(ultDerecha);
		//aparcaSobreLaDcha();
		//if (detectaFrontalIzda(10) && detectaFrontalDcha(10){}



		if (esSituacionAvanzar()){
			LED_TOP_GREEN_ON();
			accionSituacionAvanzar();
			LED_TOP_GREEN_OFF();
		} 
		else if (esSituacionParedIzda()){
			accionSituacionParedIzda();
		}
		else if (esSituacionParedDcha()){
			accionSituacionParedDcha();
		}
		else if (detectaChoqueFrontal()){
			LED_BRAKE_ON();
			aparcaSobreLaDcha();
			LED_BRAKE_OFF();
		}
		else {
			LED_TOP_RED_ON();
			accionSituacionBloqueo();
			LED_TOP_RED_OFF();
		}
	}
}



// Situaciones

unsigned char esSituacionBloqueo(){
	return (
		detectaFrontalIzda(90)
		&& detectaFrontalDcha(90)
		&& detectaLateralIzda(90)
		&& detectaLateralDcha(90)
	);
}

void accionSituacionBloqueo(){
	MOT_ROT(50, BACK ,CENTER, LEFT, ANGLE, 1);
	MOT_ROT(50, FWD ,CENTER, RIGHT, ANGLE, 1);
	while(!MOT_END){};
}

unsigned char esSituacionParedDcha(){
	return (
		detectaLateralDcha(90)
		&& !detectaLateralIzda(90)
		&& !detectaFrontalIzda(90)
		&& !detectaFrontalDcha(90)
	);
}

void accionSituacionParedDcha(){
	MOT_ROT(50, FWD ,CENTER, LEFT, ANGLE, 1);
	while(!MOT_END){};
	MOT_STR(100,FWD,TIME,3);
	while(!MOT_END){};
}

unsigned char esSituacionParedIzda(){
	return (
		!detectaLateralDcha(90)
		&& detectaLateralIzda(90)
		&& !detectaFrontalIzda(90)
		&& !detectaFrontalDcha(90)
	);
}

void accionSituacionParedIzda(){
	MOT_ROT(50, FWD ,CENTER, RIGHT, ANGLE, 1);
	while(!MOT_END){};
	MOT_STR(100,FWD,TIME,3);
	while(!MOT_END){};
}

unsigned char esSituacionAvanzar(){
	return (
		detectaLateralIzda(90)
		&& detectaLateralDcha(90)
		&& !detectaFrontalIzda(90)
		&& !detectaFrontalDcha(90)
	);
}

void accionSituacionAvanzar(){
	MOT_STR(100, FWD, TIME, 5);
	while(!MOT_END){};
}


//////////////////////////////////////////////////////////
// Detectores
//////////////////////////////////////////////////////////

unsigned char detectaFrontalIzda(unsigned char distChoque){
	if (SEN_OBS_ANALOG(OBS_CENTER_L)>distChoque){
		return 1;
	} else {
		return 0;
	}
}

unsigned char detectaFrontalDcha(unsigned char distChoque){
	if (SEN_OBS_ANALOG(OBS_CENTER_R)>distChoque){
		return 1;
	} else {
		return 0;
	}
}

unsigned char detectaLateralIzda(unsigned char distChoque){
	if (SEN_OBS_ANALOG(OBS_SIDE_L)>distChoque){
		return 1;
	} else {
		return 0;
	}
}

unsigned char detectaLateralDcha(unsigned char distChoque){
	if (SEN_OBS_ANALOG(OBS_SIDE_R)>distChoque){
		return 1;
	} else {
		return 0;
	}
}

unsigned char detectaChoqueFrontal(){
	return
		!detectaLateralIzda(90)
		&& !detectaLateralDcha(90)
		&& detectaFrontalIzda(90)
		&& detectaFrontalDcha(90)
	;
}
*/
